#include "model.h"
/*features*/
Feature::Feature() {}
Feature::Feature(int size) {
	resize(size);
}
void Feature::resize(int size) {
	_vItems.resize(size);
}
float Feature::getItem(int k) const{
	if (k>=0 && k<int(_vItems.size()))
		return _vItems[k];
	else {
		std::ostringstream s;
		s<< "Key "<<k<<" is not within range of items ("<<_vItems.size()<<")";
		WARNING(s.str());
		return 0;
	}
}
void Feature::setItem(int k, float f) {
	if (k>=0 && k<int(_vItems.size()))
		_vItems[k] = f;
	else {
		std::ostringstream s;
		s<< "Key "<<k<<" is not within range of items ("<<_vItems.size()<<")";
		WARNING(s.str());
	}
}
int Feature::getSize() const{
	return _vItems.size();
}
float Feature::getEukDistance(const Feature& f) const{
	/* std: log euklidean distance*/
	float r = 0, t = 0;
	for (int i = 0; i<getSize(); ++i) {
		t = getItem(i) - f.getItem(i);
		r+=t*t;
	}
	return sqrt(r);
}
float Feature::getDistance(const Feature& f) const{
	//own distance measure
	float euk = getEukDistance(f)/198; // sqrt(255*255*27) = 1325... = max of euk distance, /10 for realistic values
	std::ostringstream s;
	s<<"euk: "<<euk;
	euk = exp(-euk);
	s<< " exp(-euk): "<<euk;
	MAXVERBOSE(s.str());
	return euk; 
}
/* features for nearest neighbour*/
NNFeature::NNFeature() {}
NNFeature::NNFeature(const Image& i, int x, int y) {
	initialise(i,x,y);
}
void NNFeature::initialise(const Image& i, int x, int y) {
	resize(27);
	Pixel p,z;
	if (x<0 || x>=int(i.getWidth())) z = qRgb(0,0,0);
	else z = i.getPixel(x,y);
	int c = 0;
	for ( int xi = x-1; xi<=x+1; ++xi) {
		for (int yi = y-1; yi<=y+1; ++yi) {
			if (xi<0 || yi<0 || xi>=int(i.getWidth()) || yi>=int(i.getHeight())) {
				setItem(c++,qRed(z));
				setItem(c++,qGreen(z));
				setItem(c++,qBlue(z));
			} else {
				p = i.getPixel(xi,yi);
				setItem(c++,qRed(p));
				setItem(c++,qGreen(p));
				setItem(c++,qBlue(p));
			}
		}
	}
}
/*virtual model*/
Model::Model(){}
Model::~Model(){}
/* baseModel */
BaseModel::BaseModel() :_bRecalcObject(false), _fEdgeWeight(1.),_fSosiWeight(1.),_fPo(0.5),_fPb(0.5) {}
void BaseModel::train(const Trimap & t) {t.getWidth();}
Weights BaseModel::getWeights(uint xo, uint yo, const Trimap & t) {Weights w(0,0,0,0); this->getWeights(xo,yo,w,t); return w;}
void BaseModel::getWeights(uint xo, uint yo, Weights& w, const Trimap & t) {t.getWidth(); xo = 0; yo = 0; w.setTop(0); w.setLeft(0); w.setObject(0); w.setBackground(0);}
void BaseModel::setImage(Image i) {	_oImage = i;}
void BaseModel::reset() {}
void BaseModel::setEdgeWeight(float w) {
	std::ostringstream s;
	s<<"set EdgeWeigth to "<<w;
	VERBOSE(s.str());
	_fEdgeWeight = w;
}

float BaseModel::getEdgeWeight() const {
	if (_fEdgeWeight>0)
		return _fEdgeWeight;
	else
		return 0.001;
}
float BaseModel::getSosiWeight() const {
	if (_fSosiWeight>0)
		return _fSosiWeight;
	else
		return 0.001;
}
void BaseModel::setSosiWeight(float w) {
	std::ostringstream s;
	s<<"set Source/Sink Weigth to "<<w;
	VERBOSE(s.str());
	_fSosiWeight = w;
}

/* Simple Model*/
void SimpleModel::train(const Trimap & t){t.getWidth();}
void SimpleModel::getWeights(uint x, uint y, Weights &w, const Trimap & t) {
	w.setObject(255);
	w.setBackground(255);
	if (x>0)
		w.setTop(getEdgeWeight()*_oImage.getDistance(x,y,x-1,y));
	if (y>0)
		w.setLeft(getEdgeWeight()*_oImage.getDistance(x,y,x,y-1));
	switch(t.getXY(x,y)) {
	case Trimap::object:
		w.setObject(100000);
		w.setBackground(255);
		break;
	case Trimap::background:
		w.setObject(255);
		w.setBackground(100000);
		break;
	default:;
	}
}
void BaseModel::setClassProbs(float o, float b) {
	_fPo = o;
	_fPb = b;
}

/* Nearest Neighbour Model*/
NNModel::NNModel() {
	BaseModel();
	MAXVERBOSE("initialising");
}
void NNModel::reset() {
	_vBackground.resize(0);
	_vObject.resize(0);
}
void NNModel::train(const Trimap & t) {
	VERBOSE("starting training");
	for (uint x = 0; x<_oImage.getWidth(); ++x)
		for (uint y = 0; y<_oImage.getHeight(); ++y)
			if (t.getXY(x,y)==Trimap::object)
				_vObject.push_back(NNFeature(_oImage,x,y));
			else if (t.getXY(x,y)==Trimap::background)
				_vBackground.push_back(NNFeature(_oImage,x,y));
	VERBOSE("ending training");
}
void NNModel::getWeights(uint x, uint y, Weights &w, const Trimap & t){
	std::ostringstream s;
	NNFeature f(_oImage,x,y);
	float p;
	if (x>0)
		w.setTop(_fEdgeWeight*f.getDistance(NNFeature(_oImage,x-1,y)));
	if (y>0)
		w.setLeft(_fEdgeWeight*f.getDistance(NNFeature(_oImage,x,y-1)));
	switch(t.getXY(x,y)) {
	case Trimap::object:
		p = 1;
		break;
	case Trimap::background:
		p= 0;
		break;
	default:
		float	po = calcProb(f,_vObject) * _fPo,
					pb = calcProb(f,_vBackground) * _fPb;
		p = 1-po/(po+pb);
		s<< p <<" " <<po<<" "<<pb;
		MAXVERBOSE(s.str());
		break;
	}
	w.setObject(p*getSosiWeight());
	w.setBackground((1-p)*getSosiWeight());
}
float NNModel::calcProb(const NNFeature& f, const vfeature_t& v) const {
	float p = 0;
	for (uint i = 0; i<v.size();++i) {
		p+=f.getEukDistance(v[i]);
	}
	return p/v.size();
}
/*GMD-Model*/
GmdModel::GmdModel(){
	BaseModel();
	_oGmdObject = NULL;
	_oGmdBackground = NULL;
	reset();
}
GmdModel::~GmdModel() {
	delete _oGmdObject;
	delete _oGmdBackground;
}
void GmdModel::reset() {
	if (_oGmdObject!=NULL)
		delete _oGmdObject;
	if (_oGmdBackground!=NULL)
		delete _oGmdBackground;
	_oGmdObject = new GaussMixtureDensity(27);
	_oGmdBackground = new GaussMixtureDensity(27);
}
FeatureVector* GmdModel::getFeatureVector(int x, int y, const Trimap & t) const{
	FeatureVector* v = new FeatureVector;
	v->resize(27);
	QRgb p;
	int c = 0;
	v->cls() = int(t.getXY(x,y));
	for (int xi = x-1; xi<=x+1; ++xi)
	for (int yi = y-1; yi<=y+1; ++yi) {
		if (xi<0 || yi<0 || xi>=int(_oImage.getWidth()) || yi>=int(_oImage.getHeight())) {
			(*v)[c++] = 0;
			(*v)[c++] = 0;
			(*v)[c++] = 0;
		} else {
			p = _oImage.getPixel(xi,yi);
			(*v)[c++] = qRed(p);
			(*v)[c++] = qGreen(p);
			(*v)[c++] = qBlue(p);
		}
	}
	return v;
}
void GmdModel::train(const Trimap & t) {
	VERBOSE("Starting training");
	DataSet object;
	DataSet	background;
	int o = 0;
	int b = 0;
	FeatureVector *v;
	VERBOSE("Building datasets");
	for (uint x=0; x<_oImage.getWidth(); ++x)
	for (uint y=0; y<_oImage.getHeight(); ++y) {
		switch (t.getXY(x,y)) {
		case Trimap::object:
			v = getFeatureVector(x,y,t);
			assert (v);
			object.push_back(v);
			++o;
			break;
		case Trimap::background:
			v = getFeatureVector(x,y,t);
			assert (v);
			background.push_back(v);
			++b;
			break;
		default: break;
		}
	}
	std::ostringstream s;
	s<< "Training with object: "<<object.size()<<", background:"<< background.size();
	VERBOSE(s.str());
	if (object.size()>0)
		_oGmdObject->train(object);
	if (background.size()>0)
		_oGmdBackground->train(background);
	VERBOSE("Ending training");
}
float GmdModel::getObjectProb(uint x, uint y, const Trimap & t) const {
	float p,b,o;
	std::ostringstream s;
	FeatureVector* v;
	v = getFeatureVector(x,y,t);
	o = _oGmdObject->logp_x((*v));
	b = _oGmdBackground->logp_x((*v));
	p = std::max(o,b);
	s<< "o: "<<o << " b: "<<b<<" max: "<<p <<" ";
	o = exp(o-p);
	b = exp(b-p);
	s<< "o: "<<o << " b: "<<b<< " ";
	o = o/(o+b);
	s<< "testo: "<<o << " "<< o/(o+b)<< " "<<(o+b);
	b = 1.-o;
	s<< "o: "<<o << " b: "<<b<< " ";
	o *= _fPo;
	b *= _fPb;
	s<< "o: "<<o << " b: "<<b<<" po: "<<_fPo<<" pb: "<<_fPb;
	p = o/(o+b);
	s<<" pr: "<<p;
	MAXVERBOSE(s.str());
	delete v;
	return p;
}
void GmdModel::getWeights(uint x, uint y, Weights& w, const Trimap & t) {
	float p;
	switch (t.getXY(x,y)) {
	case Trimap::object:			p = 1; break;
	case Trimap::background:	p = 0; break;
	default:									p = getObjectProb(x,y,t); break;
	}
	w.setObject(p*getSosiWeight());
	w.setBackground((1-p)*getSosiWeight());
/*	w.setObject(p);
	w.setBackground(1-p);*/
	NNFeature f(_oImage,x,y);
	if (x>0)
		w.setTop(_fEdgeWeight*f.getDistance(NNFeature(_oImage,x-1,y)));
	if (y>0)
		w.setLeft(_fEdgeWeight*f.getDistance(NNFeature(_oImage,x,y-1)));
}
FannModel::FannModel() {
	reset();
}
int print_callback(FANN::neural_net &net, FANN::training_data &train,
    unsigned int max_epochs, unsigned int epochs_between_reports,
    float desired_error, unsigned int epochs, void *user_data) {
	train = train; max_epochs = max_epochs; epochs_between_reports = epochs_between_reports; desired_error = desired_error; user_data = user_data;//no warnings
	std::ostringstream s;
	s << "Epochs     " << epochs << ". "
		<< "Current Error: " << net.get_MSE();
	VERBOSE(s.str());
	return 0;
}

void FannModel::train(const Trimap & t) {
	const float desired_error = 0.001f;
	const unsigned int max_iterations = 3000;
	const unsigned int iterations_between_reports = 500;
	VERBOSE("Initialising data");
	int count = t.getCount(Trimap::object) + t.getCount(Trimap::background),
			i = 0; //counter for input/output
	FANN::training_data data;
	fann_type **	input = new fann_type*[count];
	fann_type **	output = new fann_type*[count];
	Trimap::obu obu;
	uint o=0,b = 0;
	for (uint x = 0; x<_oImage.getWidth(); ++x)
	for (uint y = 0; y<_oImage.getHeight(); ++y) {
		obu = t.getXY(x,y);
		if (obu!=Trimap::unknown) {
			input[i] = createFannType(x,y);
			output[i] = new fann_type[1];
			switch (obu) {
				case Trimap::object:			output[i][0] = 1.; ++o; break;
				case Trimap::background:	output[i][0] = 0.; ++b; break;
				default: break;
			}
			++i;
		}
	}
	data.set_train_data(count, 27,input, 1, output);
	VERBOSE("Training network");
	_oFann.init_weights(data);
	_oFann.set_callback(print_callback, NULL);
	_oFann.train_on_data(data, max_iterations,
            iterations_between_reports, desired_error);
	for (i = 0; i<count; ++i) {
		delete [] input[i];
		delete [] output[i];
	}
	delete [] input;
	delete [] output;
	VERBOSE("Done training");
}
void FannModel::getWeights(uint x, uint y, Weights& w, const Trimap & t) {
	float p,b,o;
	switch (t.getXY(x,y)) {
	case Trimap::object:			p = 1; break;
	case Trimap::background:	p = 0; break;
	default:
		fann_type * tmp;
		fann_type * v = createFannType(x,y);
		tmp = _oFann.run(v);
		delete [] v;
		o = float(tmp[0])*_fPo;
		b = float(1-o)*_fPb;
		std::ostringstream s;
		s<<"o: "<<o<<" b: "<<b;
		p = o/(o+b);
		MAXVERBOSE(s.str());
	}
	w.setObject(p*getSosiWeight());
	w.setBackground((1-p)*getSosiWeight());
	NNFeature f(_oImage,x,y);
	if (x>0)
		w.setTop(_fEdgeWeight*f.getDistance(NNFeature(_oImage,x-1,y)));
	if (y>0)
		w.setLeft(_fEdgeWeight*f.getDistance(NNFeature(_oImage,x,y-1)));
}
fann_type * FannModel::createFannType(int x, int y) {
	fann_type * v = new fann_type[27];
	int c = 0;
	Pixel p;
	for (int xi = x-1; xi<=x+1; ++xi)
	for (int yi = y-1; yi<=y+1; ++yi) {
		if (xi<0 || yi<0 || xi>=int(_oImage.getWidth()) || yi>=int(_oImage.getHeight())) {
			v[c++] = 0;
			v[c++] = 0;
			v[c++] = 0;
		} else {
			p = _oImage.getPixel(xi,yi);
			v[c++] = qRed(p);
			v[c++] = qGreen(p);
			v[c++] = qBlue(p);
		}
	}
	return v;
}
void FannModel::reset() {
	const float learning_rate = 0.7f;
	const unsigned int num_layers = 3;
	const unsigned int num_input = 27;
	const unsigned int num_hidden = 3;
	const unsigned int num_output = 1;
	_oFann.destroy();
	_oFann.create_standard(num_layers, num_input, num_hidden, num_output);
	_oFann.set_learning_rate(learning_rate);

	_oFann.set_activation_steepness_hidden(1.0);
	_oFann.set_activation_steepness_output(1.0);
	
	_oFann.set_activation_function_hidden(FANN::SIGMOID_SYMMETRIC_STEPWISE);
	_oFann.set_activation_function_output(FANN::SIGMOID_SYMMETRIC_STEPWISE);
}
/* gmd2model */
float Gmd2Model::getObjectProb(uint x, uint y, const Trimap & t) const {
	float b,o;
	std::ostringstream s;
	FeatureVector* v;
	v = getFeatureVector(x,y,t);
	o = _oGmdObject->logp_x((*v));
	b = _oGmdBackground->logp_x((*v));
	s<< "o: "<<o << " b: "<<b<<" ";
	o = 1/-o;
	b = 1/-b;
	s<< "o: "<<o << " b: "<<b<<" ";
	o = o/(o+b);
	b = 1-o;
	o*=_fPo;
	b*=_fPb;
	s<< "o: "<<o << " b: "<<b<<" po: "<<_fPo<<" pb: "<<_fPb;
	o = o/(o+b);
	s<<" pr: "<<o;
	MAXVERBOSE(s.str());
	delete v;
	return o;

}
void Gmd2Model::getWeights(uint x, uint y, Weights& w, const Trimap & t) {
	float p;
	switch (t.getXY(x,y)) {
	case Trimap::object:			p = 1; break;
	case Trimap::background:	p = 0; break;
	default:									p = getObjectProb(x,y,t); break;
	}
	w.setObject(p*getSosiWeight());
	w.setBackground((1-p)*getSosiWeight());
/*	w.setObject(p);
	w.setBackground(1-p);*/
	NNFeature f(_oImage,x,y);
	if (x>0)
		w.setTop(_fEdgeWeight*f.getDistance(NNFeature(_oImage,x-1,y)));
	if (y>0)
		w.setLeft(_fEdgeWeight*f.getDistance(NNFeature(_oImage,x,y-1)));
}
